program number5
use analyse_lattice
implicit none
integer no,np
type(real_8) y(nvd)   ! nvd=3
type(normalform) normal
type(damap) map
type(taylor) x2ave
real(dp) fix(nvd),prec,CORRECTOR(8),Circ,s,ang,total_tune
integer i,pos,k,k_theta,trf
type(pbresonance) H
type(vecresonance) F
type(TAYLOR) R
type(GMAP) G
LOGICAL MORE
      TYPE(DAMAP) ID,a1,rot
      TYPE(NORMALFORM) NORM
      TYPE(vecresonance) VECTOR
      TYPE(PBresonance) BRACKET
      TYPE(REAL_8) ya1(nvd)
      TYPE(COMPLEXTAYLOR) T,DT
      type(reversedragtfinn) rdf
prec=1.d-10
trf=1

!call make_lattice("lattice2.txt")
call make_lattice("latout.txt")

! We are allowed 3 correctors qf1,qd2,qf3,qd4,qf5,qd6,qf7,qd8

!First thing we can do: try to reduce the driving term of 4nux
! using octupoles terms 

100 continue 





! Compute One-turn map and normalize it including octupoles at qf2,qd4,qf6,qd8
 pos=1
fix=0.d0;

call find_orbit_with_fpp(lattice,pos,fix)
no=1;np=0;
call init(no,1,np,0)
call alloc(y); call alloc(normal); call alloc(map);

do i=1,2
y(i)=fix(i)+(1.d0.mono.i)
enddo


call track(y,lattice,pos)
map%v(1)=y(1);map%v(2)=y(2);

normal=map

total_tune=normal%tune(1)+2.d0
k_theta=7

write(6,*) "total tune ",total_tune
OPEN(UNIT=21,FILE='TEMP')
CALL PRINT(normal%a_t,21,prec)
CLOSE(21)

call kill(y); call kill(normal); call kill(map);



knob=.false.
call locate_set_knobs(2)
knob=.true.

no=3;np=8; pos=1

call init(no,1,np,0)
       CALL ALLOC(Y);CALL ALLOC(Ya1); CALL ALLOC(ID,a1);CALL ALLOC(NORM);
       CALL ALLOC(BRACKET); CALL ALLOC(VECTOR); CALL ALLOC(rdf);
       call alloc(rot);  CALL ALLOC(T,DT); call alloc(g);call alloc(r);

circ=lattice%magnet(lattice%n)%twiss(0)
write(6,*) " circumference = ",circ
       open(unit=20,file="temp")
       call read(a1,20)
       close(20)

       id=1

       ya1(1)=FIX(1)+a1%v(1);ya1(2)=FIX(2)+a1%v(2);
       y(1)=FIX(1)+id%v(1);y(2)=FIX(2)+id%v(2);
       T=0.D0
       s=0.d0
       do i=1,lattice%n
        CALL TRACK(y,lattice,i,i+1)

 
        ang= -twopi*S*total_tune/circ
        rot%v(1)=cos(ang)*(1.d0.mono.1) + sin(ang)*(1.d0.mono.2)
        rot%v(2)=cos(ang)*(1.d0.mono.2) - sin(ang)*(1.d0.mono.1)
        if(k_theta==0) rot=1
        A1=ya1
        A1=A1.SUB.1
        A1=A1*ROT

      if(trf==1) then
        id=y   ! Map for element i
        id=a1**(-1)*id*a1
        rdf=id   ! Compute reversed Dragt-Finn  Map= Fexp(F.grad) Linear&&
        VECTOR=rdf%NONLINEAR
      else      
        id=y   ! Map for element i
        rdf=id   ! Compute reversed Dragt-Finn  Map= Fexp(F.grad) Linear&
        rdf%NONLINEAR=A1*rdf%NONLINEAR
        VECTOR=rdf%NONLINEAR
      endif       
         DT%R=VECTOR%cos%v(1)<='02'
         DT%I=VECTOR%sin%v(1)<='02'
         T=T+EXP(I_*k_theta*twopi*S/CIRC)*DT
        ! call print(t,6)
		!  pause 7
        
        CALL TRACK(Ya1,lattice,i,i+1)   ! Tracking A_0   : A_0 M_0s 

      s=s+lattice%magnet(i)%l
        FIX(1)=YA1(1).sub.'0'; FIX(2)=YA1(2).sub.'0'
        ID=1
        y(1)=FIX(1)+id%v(1);y(2)=FIX(2)+id%v(2);

        
       enddo
call print(t,6)
       id=1
       y(1)=FIX(1)+id%v(1);y(2)=FIX(2)+id%v(2);
        CALL TRACK(y,lattice,1)
		id=y
       open(unit=20,file="temp")
       call read(a1,20)
       close(20)
        id=a1**(-1)*id*a1
		rdf=id
        VECTOR=rdf%NONLINEAR
        call print(vector,6)
PAuse

DO I=1,8
  G%V(I)= (1.D0.MONO.I)
   G%V(I)=G%V(I)+ (1.D0.MONO.(8+1))*(T%r.D.I)+ (1.D0.MONO.(8+2))*(T%i.D.I)
ENDDO

R=0.D0
DO I=1,8
R=R+(1.D0.MONO.I)
ENDDO
R=R
!R(1)=0.D0
DO I=1,8
 G%V(I)= G%V(I)-R
ENDDO

G%V(8+1)=t%r
G%V(8+2)=t%i

G=G.OO.(-1)

DO I=1,8
CORRECTOR(I)=G%V(I).SUB.'0'
ENDDO
WRITE(6,*) CORRECTOR
CALL locate_POWER_knobs(CORRECTOR)



 CALL kill(Y);CALL kill(Ya1); CALL kill(ID,a1);CALL kill(NORM);
 CALL kill(BRACKET); CALL kill(VECTOR); CALL kill(rdf);
 call kill(rot);  CALL kill(T,DT); call kill(g);call kill(r);


WRITE(6,*) " MORE ITERATIONS "
READ(5,*) MORE

IF(MORE) GOTO 100
call write_lattice("lattice5.txt")




end program number5

subroutine locate_set_knobs(npara)
use analyse_lattice
implicit none
integer i,npara,k,BN
k=0
BN=3
do i=1,lattice%n

select case(lattice%magnet(i)%name(1:3))

case('QF1')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+1
case('QD2')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+2
case('QF3')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+3
case('QD4')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+4
case('QF5')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+5
case('QD6')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+6
case('QF7')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+7
case('QD8')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+8
case default
k=k+1
end select
enddo 

write(6,*)lattice%n-k, " knobs found "
end subroutine locate_set_knobs

subroutine locate_POWER_knobs(C)
use analyse_lattice
implicit none
REAL(DP) C(8)
integer i,npara,k,BN
k=0
BN=3
do i=1,lattice%n

select case(lattice%magnet(i)%name(1:3))

case('QF1')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(1)+lattice%magnet(i)%k(BN)%R
case('QD2')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(2)+lattice%magnet(i)%k(BN)%R
case('QF3')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(3)+lattice%magnet(i)%k(BN)%R
case('QD4')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(4)+lattice%magnet(i)%k(BN)%R
case('QF5')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(5)+lattice%magnet(i)%k(BN)%R
case('QD6')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(6)+lattice%magnet(i)%k(BN)%R
case('QF7')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(7)+lattice%magnet(i)%k(BN)%R
case('QD8')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(8)+lattice%magnet(i)%k(BN)%R
case default
k=k+1
end select
enddo 

write(6,*)lattice%n-k, " knobs found "
end subroutine locate_POWER_knobs
